package es.uji.viselab.experiment;

import es.uji.viselab.image.process.ImageProcessing;
import es.uji.viselab.image.process.cameras.VirtualCameraParameter;
import es.uji.viselab.math.Matrix4;
import es.uji.viselab.math.Vector2;
import es.uji.viselab.math.Vector3;
import es.uji.viselab.network.DataPackage;
import es.uji.viselab.robot.RobotException;

public class Experiment3_Customizable_VirtualCamera extends Experiment {

	int iterations;
	double[] velocities;
	private ModelCreator object3d;

	public Experiment3_Customizable_VirtualCamera() {
		this.camParams = new VirtualCameraParameter();
		object3d = new ModelCreator();
		object3d.setCamera(camParams);
		object3d.setEye(1.0,0.0,0.01);
		object3d.setLootAt(0,0,-1);
	}
	
	@Override
	public void newConnectionStablished() {
		// TODO Auto-generated method stub
	}

	@Override
	public void newDataArrived(DataPackage dn) {
		benchmark.getDatabase().insert(dn);
	}

	@Override
	public void cameraFeaturePointsChanged(ImageProcessing i) {
		if (visualServoing != null) {
			// Velocities are based on camera frame coordinates
			double[] velocitiesCameraFrame = visualServoing.compute(i);
		    Vector3 velocitiesCameraFrameVector = new Vector3(velocitiesCameraFrame[0],velocitiesCameraFrame[1],velocitiesCameraFrame[2]);
			
			// Transformation End effector --> Camera Frame
			Matrix4 cameraPose = camParams.getPose();
			//Transformation Camera frame --> End effector
			cameraPose = cameraPose.getInverseHomogenousTransformation();
			
			// Transformation World Coordinate --> End effector
			Matrix4 robotPose = robot.getPoseM();
			// Transformation End effector --> World Coordinate
			robotPose = robotPose.getInverseHomogenousTransformation();
			
			Matrix4 pose = new Matrix4();
			pose.mul(robotPose,cameraPose);
			pose.m03=0;
			pose.m13=0;
			pose.m23=0;
			Vector3 velocitiesEndEffector = pose.mul(velocitiesCameraFrameVector);
			velocities = new double[6];
		    velocities[0]=velocitiesEndEffector.x;
		    velocities[1]=velocitiesEndEffector.y;
		    velocities[2]=0;//velocitiesEndEffector.z;
		    velocities[3]=0;
		    velocities[4]=0;
		    velocities[5]=0;
			System.out.println("Displacement end-effector requested: " + velocitiesEndEffector.x + ", "+ velocitiesEndEffector.y+ ", "+velocitiesEndEffector.z);
		}
	}

	@Override
	public void Start() {
		iterations = 0;
        velocities = new double[6];
		for (int i = 0; i < 6; i++)
			velocities[i] = 0;
		
		while (iterations < 50) {
			System.out.println("Iteration " + iterations );
			try {
				robot.move(velocities);
			} catch (RobotException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			iterations++;
		}
		System.out.println("END CONTROL LOOP AT " + iterations + " ITERATIONS ");
	}

	@Override
	public Vector3[] getModel3d() {
		 
		//Vector3[] points = object3d.getSquare3d();
		Vector3[] points = getSquare();
		return points;
	}

	private Vector3[] getSquare() {
		Vector3 p1 = new Vector3(1.05 + 0.1, 0.05 + 0.1, 0);
		Vector3 p2 = new Vector3(1.05 + 0.1, -0.05 + 0.1, 0);
		Vector3 p3 = new Vector3(0.95 + 0.1, -0.05 + 0.1, 0);
		Vector3 p4 = new Vector3(0.95 + 0.1, 0.05 + 0.1, 0);
		Vector3[] points = { p2, p3, p4, p1 };
		return points;
	}

	@Override
	public Vector2[] getTarged2d() {
		
		//Vector2[] points  = object3d.getSquare2d();
		Vector2 p1 = new Vector2(100, 100);
		Vector2 p2 = new Vector2(100, -100);
		Vector2 p3 = new Vector2(-100, -100);
		Vector2 p4 = new Vector2(-100, 100);
		Vector2[] points = { p1, p2, p3, p4 };

		return points;
	}

}
